/**
 * \file
 * \date 18.09.2013
 * \author alex
 */
#include <bullet_rviz_test/robot.h>
#include <bullet/btBulletCollisionCommon.h>
#include <bullet/btBulletDynamicsCommon.h>

namespace bullet_rviz_test
{

Robot::Robot()
{
  btVector3 robot_half_extents(0.35, 0.30, 0.15);
  //btVector3 robot_half_extents(0.05, 0.05, 0.05);
  collision_shape_ = new btBoxShape(robot_half_extents);
  motion_state_ = new MotionState(this,
      btTransform(btQuaternion(0, 0, 0, 1), btVector3(0, 0, 1)));
  btScalar robot_mass = 20; // kg
  btVector3 robot_inertia;
  collision_shape_->calculateLocalInertia(robot_mass, robot_inertia);
  btRigidBody::btRigidBodyConstructionInfo rigid_body_construction_info(
      robot_mass, motion_state_, collision_shape_, robot_inertia);
  rigid_body_construction_info.m_friction = 1.7;
  rigid_body_ = new btRigidBody(rigid_body_construction_info);
}

Robot::~Robot()
{
  delete motion_state_;
  delete rigid_body_;
  delete collision_shape_;
}

btRigidBody * Robot::getRigidBody()
{
  return rigid_body_;
}

btVector3 Robot::getHalfExtents()
{
  return dynamic_cast<btBoxShape *>(collision_shape_)->getHalfExtentsWithoutMargin();
}

}
